"""
#===============================================================================
# License
#===============================================================================
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

#===============================================================================
Copyright (c) 2009 bryan p. conrad
bconrad2@gmail.com

Repository Path:    
$HeadURL$

Created on:           Oct 21, 2009
Last Revised on:      $Date$
Version:              $Revision$
ID:                   $Id$
Author:               $Author$ :: (bryan)

#===============================================================================


#===============================================================================
# Documentation
#===============================================================================

"""

#===============================================================================
# Imports
#===============================================================================
import JT
import numpy as np
import pylab
from marker_data_read import marker_data_read
import cPickle as pickle
import os
import vtk

#===============================================================================
# Spine1
#===============================================================================


#===============================================================================
# Spine2
#===============================================================================
def spine2_analysis():
    # Joint Angle Calculations
    data = spine2_setup_data_dict()
    
    JT.Kinematics.jt_anatomic_joint_angles(data,save_name,plot=False)

class AbstractData(object):
    # Results
    jt_results = {}
    jt_results[0] = {}
    jt_results[1] = {}
    jt_results[2] = {}
    
    mocap_results = {}
    mocap_results[0] = {}
    mocap_results[1] = {}
    mocap_results[2] = {}
    
#    error_angle = {}
#    error_angle[0] = {}    
#    error_angle[1] = {}
#    error_angle[2] = {}
    
    errors = {}
    errors[0] = {}
    errors[1] = {}
    errors[2] = {}
    
    rms = {}
    rms[0] = {}
    rms[1] = {}
    rms[2] = {}
    
    bias = {}
    bias[0] = {}
    bias[1] = {}
    bias[2] = {}
    
    prec = {}
    prec[0] = {}
    prec[1] = {}
    prec[2] = {}
  
#    rms_angle = {}
#    rms_angle['FE'] = {}
#    rms_angle['LB'] = {}
#    rms_angle['AR'] = {}
      

class test_data_class(AbstractData):
    junk = np.pi
    name = 'bpc'

class spine1_data(object):
    """
    Create a data class with all of the parameters that are 
    specific to each specimen.
    """
    jt_project_list = []
    jt_project_list.append(r'C:/Users/bryan/phd/data/Spine1/Spine1_1/Spine1_1_C4-C5_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine1\Spine1_5\Spine1_5_C4-C5_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine1\Spine1_8\Spine1_8_C4-C5_initial_poses.xml')
    
    jt_cluster_project = r''
    
    trc_path = []
    trc_path.append(r'J:\CONRAD\Research\Data\Spine_072106\Processed\Spine1_1v.trc')
    trc_path.append(r'J:\CONRAD\Research\Data\Spine_072106\Processed\Spine1_5v.trc')
    trc_path.append(r'J:\CONRAD\Research\Data\Spine_072106\Processed\Spine1_8v.trc')
    
    volume_path = []
    volume_path.append(r'C:\Users\bryan\phd\data\Spine1\CT\Spine1_C4_uchar_crop.mhd')
    volume_path.append(r'C:\Users\bryan\phd\data\Spine1\CT\Spine1_C5_uchar_crop.mhd')
    
    jt_output_name = r'C:\Users\bryan\phd\data\Spine1\Spine1_output.dat'
    results_path = r'C:\Users\bryan\phd\data\Analysis'
    
    trial_props = []
    trial1 = {}
    trial1['name'] = 'Spine1_1'
    trial1['specimen'] = 'Spine1'
    trial1['dir'] = 'Flexion-Extension'
    trial_props.append(trial1)
    
    trial2 = {}
    trial2['name'] = 'Spine1_5'
    trial2['specimen'] = 'Spine1'
    trial2['dir'] = 'Lateral Bending'
    trial_props.append(trial2)
    
    trial3 = {}
    trial3['name'] = 'Spine1_8'
    trial3['specimen'] = 'Spine1'
    trial3['dir'] = 'Axial Rotation'
    trial_props.append(trial3)
                           
    # Anatomic Landmarks in CT coordinates (p1=Ant, p2=Left, p3=Right)
    # Points 1 = superior vertebrae, Points 2 = inferior vertebrae
    points1 = []
    points2 = []
    landmark_list = []
    # C6
#    points1.append(np.asarray((33.96,47.58,12.75)))
#    points1.append(np.asarray((19.69,37.9,12)))
#    points1.append(np.asarray((43.64,31.99,12.5)))

    points1.append(np.asarray((33.96,2.63,12.75)))
    points1.append(np.asarray((19.69,12.3,12)))
    points1.append(np.asarray((43.64,18.21,12.5)))
    landmark_list.append(points1)
    # C7
#    points2.append(np.asarray((39.38,45.94,11.5)))
#    points2.append(np.asarray((22.8,35.11,11.5)))
#    points2.append(np.asarray((50.2,28.71,11.5)))

    points2.append(np.asarray((39.38,10.01,11.5)))
    points2.append(np.asarray((22.8,20.84,11.5)))
    points2.append(np.asarray((50.2,27.23,11.5)))
    landmark_list.append(points2)
    # list of markers attached to vertebrae
    # order of indices is: Left, Right, Anterior 
    marker_list = []
    marker_list.append([9,7,8]) # Spine1-C4
    marker_list.append([12,10,11]) # Spine1-C5
    
     # Volume centers are used as origin of volume in pyTrack
    # Like landmarks, center[0] is superior, center[1] is inferior 
    centers = []
    centers.append((29.44921875, 25.01953125, 16.5))
    centers.append((36.421875, 27.890625, 14.5))
    
#    # Sync Frames from fluoro data
#    sync_fluoro = []
#    sync_fluoro.append(16)
#    sync_fluoro.append(56)
#    sync_fluoro.append(64)
#    
#    # Sync Frames from mocap data
#    sync_mocap = []
#    sync_mocap.append(52)
#    sync_mocap.append(29)
#    sync_mocap.append(37)
    
    ##########################################
    # From linear regression synchronization
    # Sync Frames from fluoro data
    sync_fluoro = []
    sync_fluoro.append(0)
    sync_fluoro.append(23)
    sync_fluoro.append(24)
    
    # Sync Frames from mocap data
    sync_mocap = []
    sync_mocap.append(31)
    sync_mocap.append(0)
    sync_mocap.append(0)
    ############################################
    
    # Results
    jt_results = {}
    jt_results[0] = {}
    jt_results[1] = {}
    jt_results[2] = {}
    
    mocap_results = {}
    mocap_results[0] = {}
    mocap_results[1] = {}
    mocap_results[2] = {}
    
#    error_angle = {}
#    error_angle[0] = {}    
#    error_angle[1] = {}
#    error_angle[2] = {}
    
    errors = {}
    errors[0] = {}
    errors[1] = {}
    errors[2] = {}
    
    rms = {}
    rms[0] = {}
    rms[1] = {}
    rms[2] = {}
    
    bias = {}
    bias[0] = {}
    bias[1] = {}
    bias[2] = {}
    
    prec = {}
    prec[0] = {}
    prec[1] = {}
    prec[2] = {}
    
    jt_joint_angles = {}
    jt_joint_angles[0] = {}
    jt_joint_angles[1] = {}
    jt_joint_angles[2] = {}

    mocap_joint_angles = {}
    mocap_joint_angles[0] = {}
    mocap_joint_angles[1] = {}
    mocap_joint_angles[2] = {}


class spine2_data(object):
    """
    Create and return a data dictionary with all of the parameters that are 
    specific to each specimen.
    """
    jt_project_list = []
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine2\Spine2_2\Spine2_2_C6-C7_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine2\Spine2_5\Spine2_5_C6-C7_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine2\Spine2_9\Spine2_9_C6-C7_initial_poses.xml')
    
    trial_props = []
    trial1 = {}
    trial1['name'] = 'Spine2_2'
    trial1['specimen'] = 'Spine2'
    trial1['dir'] = 'Flexion-Extension'
    trial_props.append(trial1)
    
    trial2 = {}
    trial2['name'] = 'Spine2_5'
    trial2['specimen'] = 'Spine2'
    trial2['dir'] = 'Lateral Bending'
    trial_props.append(trial2)
    
    trial3 = {}
    trial3['name'] = 'Spine2_9'
    trial3['specimen'] = 'Spine2'
    trial3['dir'] = 'Axial Rotation'
    trial_props.append(trial3)
    
    jt_cluster_project = r'C:\Users\bryan\phd\data\Spine2\Spine2_C6C7_cluster.xml'
    
    trc_path = []
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine2_2.trc')
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine2_5.trc')
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine2_9.trc')
    
    volume_path = []
    volume_path.append(r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C6_uchar_crop.mhd')
    volume_path.append(r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C7_uchar_crop.mhd')
    
    jt_output_name = r'C:\Users\bryan\phd\data\Spine2\Spine2_2_output.dat'
    results_path = r'C:\Users\bryan\phd\data\Analysis'
    
    # Anatomic Landmarks in CT coordinates (p1=Ant, p2=Left, p3=Right)
    # Points 1 = superior vertebrae, Points 2 = inferior vertebrae
    points1 = []
    points2 = []
    landmark_list = []
    # C6
    points1.append(np.asarray((32.12,7.26,17.75)))
    points1.append(np.asarray((22.62,32.12,18)))
    points1.append(np.asarray((51.67,27.09,16.5)))
    landmark_list.append(points1)
    # C7
    points2.append(np.asarray((32.4,6.14,15.5)))
    points2.append(np.asarray((21.79,27.65,13.5)))
    points2.append(np.asarray((50.55,21.51,15.5)))
    landmark_list.append(points2)
    # list of markers attached to vertebrae 
    marker_list = []
    marker_list.append([15,13,14]) # Spine2-C6
    marker_list.append([18,16,17]) # Spine2-C7
    
    # Volume centers are used as origin of volume in pyTrack
    # Like landmarks, center[0] is superior, center[1] is inferior 
    centers = []
    centers.append((35.051765292882919, 33.794929087162018, 20.5))
    centers.append((35.051765292882919, 33.794929087162018, 19.75))
    
#    # Sync Frames
#    sync_fluoro = []
#    sync_fluoro.append(64)
#    sync_fluoro.append(48)
#    sync_fluoro.append(56)
#    
#    # Sync Frames from mocap data
#    sync_mocap = []
#    sync_mocap.append(9)
#    sync_mocap.append(52)
#    sync_mocap.append(38)

    #########################################
    # From linear regression synchronization
    # Sync Frames
    sync_fluoro = []
    sync_fluoro.append(52)
    sync_fluoro.append(0)
    sync_fluoro.append(15)
    
    # Sync Frames from mocap data
    sync_mocap = []
    sync_mocap.append(0)
    sync_mocap.append(6)
    sync_mocap.append(0)
    #########################################
    
    # Results
    jt_results = {}
    jt_results[0] = {}
    jt_results[1] = {}
    jt_results[2] = {}
    
    mocap_results = {}
    mocap_results[0] = {}
    mocap_results[1] = {}
    mocap_results[2] = {}
    
#    error_angle = {}
#    error_angle[0] = {}    
#    error_angle[1] = {}
#    error_angle[2] = {}
    
    errors = {}
    errors[0] = {}
    errors[1] = {}
    errors[2] = {}
    
    rms = {}
    rms[0] = {}
    rms[1] = {}
    rms[2] = {}
    
    bias = {}
    bias[0] = {}
    bias[1] = {}
    bias[2] = {}
    
    prec = {}
    prec[0] = {}
    prec[1] = {}
    prec[2] = {}
    
    jt_joint_angles = {}
    jt_joint_angles[0] = {}
    jt_joint_angles[1] = {}
    jt_joint_angles[2] = {}

    mocap_joint_angles = {}
    mocap_joint_angles[0] = {}
    mocap_joint_angles[1] = {}
    mocap_joint_angles[2] = {}



class spine3_data(object):
    """
    Create and return a data dictionary with all of the parameters that are 
    specific to each specimen.
    """
    
    jt_project_list = []
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine3\Spine3_1\Spine3_1_C3-C4_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine3\Spine3_5\Spine3_5_C3C4.xml')
    #jt_project_list.append(r'C:\Users\bryan\phd\data\Spine3\Spine3_7\Spine3_7_C3-C4_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine3\Spine3_7\Spine3_7_C3-C4_initial_poses.xml')
    
    jt_cluster_project = r''
    
    trc_path = []
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine3_1.trc')
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine3_5.trc')
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine3_7.trc')
    
    volume_path = []
    volume_path.append(r'C:\Users\bryan\phd\data\Spine3\CT\Spine3_C3_uchar_crop2.mhd')
    volume_path.append(r'C:\Users\bryan\phd\data\Spine3\CT\Spine3_C4_uchar_crop2.mhd')
    
    jt_output_name = r'C:\Users\bryan\phd\data\Spine3\Spine3_output.dat'
    results_path = r'C:\Users\bryan\phd\data\Analysis'
    
    trial_props = []
    trial1 = {}
    trial1['name'] = 'Spine3_1'
    trial1['specimen'] = 'Spine3'
    trial1['dir'] = 'Flexion-Extension'
    trial_props.append(trial1)
    
    trial2 = {}
    trial2['name'] = 'Spine3_5'
    trial2['specimen'] = 'Spine3'
    trial2['dir'] = 'Lateral Bending'
    trial_props.append(trial2)
    
    trial3 = {}
    trial3['name'] = 'Spine3_7'
    trial3['specimen'] = 'Spine3'
    trial3['dir'] = 'Axial Rotation'
    trial_props.append(trial3)
                           
    # Anatomic Landmarks in CT coordinates (p1=Ant, p2=Left, p3=Right)
    # Points 1 = superior vertebrae, Points 2 = inferior vertebrae
    points1 = []
    points2 = []
    landmark_list = []
    # C6
    points1.append(np.asarray((38.09,18.95,11.5)))
    points1.append(np.asarray((19.34,24.36,13)))
    points1.append(np.asarray((40.8,38.09,10.5)))
    landmark_list.append(points1)
    # C7
    points2.append(np.asarray((40.22,16.82,11.5)))
    points2.append(np.asarray((21.27,24.17,12.5)))
    points2.append(np.asarray((42.15,39.06,10.5)))
    landmark_list.append(points2)
    # list of markers attached to vertebrae 
    marker_list = []
    marker_list.append([6,4,5]) # Spine3-C3
    marker_list.append([9,7,8]) # Spine3-C4
    
    # Volume centers are used as origin of volume in pyTrack
    # Like landmarks, center[0] is superior, center[1] is inferior 
    centers = []
    centers.append((31.2275390625, 29.2939453125, 16.5))
    centers.append((31.2275390625, 29.2939453125, 16.0))
    
#    # Sync Frames
#    sync_fluoro = []
#    sync_fluoro.append(56)
#    sync_fluoro.append(48)
#    sync_fluoro.append(56)
#    
#    # Sync Frames from mocap data
#    sync_mocap = []
#    sync_mocap.append(36)
#    sync_mocap.append(43)
#    sync_mocap.append(48)
#    
    #########################################
    # From linear regression synchronization
    # Sync Frames
    sync_fluoro = []
    sync_fluoro.append(13)
    sync_fluoro.append(16)
    sync_fluoro.append(8)
    
    # Sync Frames from mocap data
    sync_mocap = []
    sync_mocap.append(0)
    sync_mocap.append(0)
    sync_mocap.append(0)
    #########################################
    
    # Results
    jt_results = {}
    jt_results[0] = {}
    jt_results[1] = {}
    jt_results[2] = {}
    
    mocap_results = {}
    mocap_results[0] = {}
    mocap_results[1] = {}
    mocap_results[2] = {}
    
#    error_angle = {}
#    error_angle[0] = {}    
#    error_angle[1] = {}
#    error_angle[2] = {}
    
    errors = {}
    errors[0] = {}
    errors[1] = {}
    errors[2] = {}
    
    rms = {}
    rms[0] = {}
    rms[1] = {}
    rms[2] = {}
    
    bias = {}
    bias[0] = {}
    bias[1] = {}
    bias[2] = {}
    
    prec = {}
    prec[0] = {}
    prec[1] = {}
    prec[2] = {}
    
    jt_joint_angles = {}
    jt_joint_angles[0] = {}
    jt_joint_angles[1] = {}
    jt_joint_angles[2] = {}

    mocap_joint_angles = {}
    mocap_joint_angles[0] = {}
    mocap_joint_angles[1] = {}
    mocap_joint_angles[2] = {}

        

def test_show_landmarks():
    data2 = spine2_data()
    print "Spine1 Volume 1"
    vol_list = [data2.volume_path[0],r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C6_mrks.stl']
    mkr_pose = [ 6.27958784e+01,  -2.16506519e+01,  -2.69692312e+01,   -4.55020544e-02,   2.96033069e+00,   1.49899738e+00]
    v1_pose = [35.05176544,  33.7949295 ,  20.5,0,0,0]
    vol_poses = [v1_pose,mkr_pose]
    JT.Kinematics.show_landmarks(vol_list, 
                                 data2.landmark_list[0],vol_poses,
                                 show_nums=True)
#    print "Spine1 Volume 2"
#    JT.Kinematics.show_landmarks(data1.volume_path[1], data1.landmark_list[1])
#    data2 = spine2_data()
#    print "Spine2 Volume 1"
#    JT.Kinematics.show_landmarks(data2.volume_path[0], data2.landmark_list[0])
#    print "Spine2 Volume 2"
#    JT.Kinematics.show_landmarks(data2.volume_path[1], data2.landmark_list[1])
#    data3 = spine3_data()
#    print "Spine3 Volume 1"
#    JT.Kinematics.show_landmarks(data3.volume_path[0], data3.landmark_list[0])
#    print "Spine3 Volume 2"
#    JT.Kinematics.show_landmarks(data3.volume_path[1], data3.landmark_list[1])
#    
def create_cluster_model():
    """
    create a cluster surface model of all visible markers in a trc file
    and save it as an stl file.
    """
    trc_path = r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine2_2c.trc'
    # Make Cluster of all markers
    model_output_path1 = r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_2_ALL_mrks.stl'
    JT.utils.make_marker_stl(trc_path, model_output_path1, marker_size=5,
                    marker_list=[], frame_range=[9,10], show=False)
    
    # Spine2-C6: [13,14,15]
    model_output_path2 = r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C6_mrks.stl'
    JT.utils.make_marker_stl(trc_path, model_output_path2, marker_size=5,
                    marker_list=[13,14,15], frame_range=[9,10], show=False)
    
    # Spine2-C6: [13,14,15]
    model_output_path3 = r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C7_mrks.stl'
    JT.utils.make_marker_stl(trc_path, model_output_path3, marker_size=5,
                    marker_list=[16,17,18], frame_range=[9,10], show=False)

def calculate_jt_anatomic_joint_angles(plot=False,save=False):
    """
    This function will load the data from the data classes and pass it to
    JT.Kinematics.jt_anatomic_joint_angles.
    
    It will then compile all the results into one array, which will be 
    returned as a numpy format array.
    """
    data_list = []
    data_list.append(spine1_data())
    data_list.append(spine2_data())
    data_list.append(spine3_data())
    
    all_joint_angles = []
    all_vert1_angles = []
    all_vert2_angles = []
    #save_name3=data3.jt_project[0].replace()
    for data in data_list:
        [joint_angles,vert1_angles,vert2_angles] = JT.Kinematics.jt_anatomic_joint_angles(data,'01',
                                               save_data=save,
                                               plot=plot)
        all_joint_angles.append(joint_angles)
        all_vert1_angles.append(vert1_angles)
        all_vert2_angles.append(vert2_angles)
        
    #all_joint_angles = np.asarray(all_joint_angles)
    nmax = 0
    for angles in all_joint_angles:        
        n = angles.shape[1]
        if n>nmax: nmax=n
    output_joint_angles = np.zeros((3,len(all_joint_angles),nmax,6))
    output_vert1_angles = np.zeros((3,len(all_vert1_angles),nmax,6))
    output_vert2_angles = np.zeros((3,len(all_vert2_angles),nmax,6))
    for spine in range(3):
        # Iterate over each specimen
        for i,angles in enumerate(all_joint_angles[spine]):
            # Iterate over each testing direction
            output_joint_angles[spine,i,0:len(angles),:] = angles
            output_vert1_angles[spine,i,0:len(angles),:] = all_vert1_angles[spine][i]
            output_vert2_angles[spine,i,0:len(angles),:] = all_vert2_angles[spine][i]
    
    # Format of output_angles [spine,testing dir,mocap frame,dim]
    return [output_joint_angles,output_vert1_angles,output_vert2_angles]


def calculate_vertebral_kinematics_jt(show=True,visual=False):
    """
    This function calculates the kinematics for each vertebral body from 
    pyTrack data.
    
    Inputs are retrieved from the data classes (for specimens 1,2,3)
    output array is a numpy format [3x3x2xNx6] -> [Spine,Trial,Vert,Frame,Dimension]
    
    show is a bool - if True, then the 3x3 plots of the data will be displayed
    """
    # Load jointtack data
    data=[]
    data.append(spine1_data())
    data.append(spine2_data())
    data.append(spine3_data())
    max_frame = 0
    if visual:
        ren = vtk.vtkRenderer()
        renWin = vtk.vtkRenderWindow()
        iren = vtk.vtkRenderWindowInteractor()
        iren.SetRenderWindow(renWin)
        style = vtk.vtkInteractorStyleTrackballCamera()
        iren.SetInteractorStyle(style)
        renWin.AddRenderer(ren)

    all_angles = []
    for vert in range(2):
        for i, specimen in enumerate(data):
            # iterate over each specimens data class
            all_angles.append([])
            
            # Calculate volume->anatomic transformation
            # Marker order in data class: p1=Ant, p2=Left, p3=Right
            # Rearrange to match what build local expects
            P1_static = np.asarray(specimen.landmark_list[vert][1])
            P2_static = np.asarray(specimen.landmark_list[vert][2])
            P3_static = np.asarray(specimen.landmark_list[vert][0])
            # Build local expects Left, Right, Anterior
            T_v0_a = JT.Kinematics.build_local(P1_static, P2_static, P3_static,z_up=False)
            
            # Calculate volume->volume center transformation
            center = specimen.centers[vert]
            T_v0_center = JT.Kinematics.get_transform([center[0],center[1],center[2],
                                                  0,0,0], '213')
            
            for j,prj in enumerate(specimen.jt_project_list):
                # iterate over each pyTrack trial
                jt_data = JT.Data2.DataStructure()
                jt_data.load(prj)
                poses = jt_data.get_all_poses()
                if poses.shape[0]>46:
                    # if there is a calibration frame, do not use it.
                    poses = poses[1:,:,:]
                
                # Interpolate poses to match mocap data collection rate
                # Flouroscopic data collection rate   
                poses_interp = JT.Kinematics.interpolate_poses(poses,7.5,60)

                # Calculate second pose transformation  (first might be calibration)
                T_f_center_1 = JT.Kinematics.get_transform(poses[1,vert,:], '213')
                T_f_v0 = np.dot(T_f_center_1,np.linalg.inv(T_v0_center))
                T_f_a = np.dot(T_f_v0,T_v0_a)
                
                # Calculate Static Transformation 
                T_static = np.dot(T_f_center_1,np.dot(np.linalg.inv(T_v0_center),T_v0_a))
                
                # Visualization Stuf
                if visual:
                    volume_path = specimen.volume_path[vert]
                    print "Visualizing: %s" % str(volume_path)
                    volume = JT.Volume2()
                    volume.SetOrigin(center)
                    volume.SetVolumeFileName(volume_path)
                    origin_marker = JT.utils.create_sphere(5, 'orange')
                    anatomy_marker = JT.utils.create_sphere(5, 'blue')
                    volume_marker = JT.utils.create_sphere(5, (1,1,1))

                    T_f_v0_vtk = JT.utils.GetVTKMatrixFromNumpyArray(T_f_v0)
                    T_f_center_1_vtk = JT.utils.GetVTKMatrixFromNumpyArray(T_f_center_1)
                    T_f_a_vtk = JT.utils.GetVTKMatrixFromNumpyArray(T_f_a)
                    volume.SetUserMatrix(T_f_v0_vtk)
                    origin_marker.SetUserMatrix(T_f_v0_vtk)
                    anatomy_marker.SetUserMatrix(T_f_a_vtk)
                    volume_marker.SetUserMatrix(T_f_center_1_vtk)
                    
                    ren.AddVolume(volume)
                    ren.AddActor(origin_marker)
                    ren.AddActor(anatomy_marker)
                    ren.AddActor(volume_marker)
                    renWin.Render()
                    iren.Start()

                all_angles[i].append([])
                print "Processing: %s" % prj
                n_frames = len(poses_interp)
                if max_frame < n_frames:
                    max_frame = n_frames
            
                # Angles is really transfromations and angles
                angles = []
                for k in range(len(poses_interp)):
                    # Iterate over each frame in mocap data
                    # Calculate current frame pose transformation
                    T_f_center = JT.Kinematics.get_transform(poses_interp[k,vert,:], '213')
                    T_motion = np.dot(T_f_center,np.dot(np.linalg.inv(T_v0_center),T_v0_a))
                    
                    T_static_motion = np.dot(np.linalg.inv(T_static),T_motion)
                    # Find euler angles using 213 [VTK] sequence
                    #tmp = JT.Kinematics.get_pose(T_static_motion, '213')
                    
                    tmp = JT.Kinematics.get_pose(T_static_motion,'213')
                    
                    #*** Change sign of translations to match mocap ***
                    tmp[0:3] = -tmp[0:3]
                    #**************************************************
                    angles.append(tmp)
                
                all_angles[i][j].append(np.asarray(angles))
                #pylab.plot(angles)
                #pylab.show()
    
    #all_angles[i][j][vert].append(np.asarray(angles))
    # All calculations are done, put body_angles into an array
    output_data = np.zeros((3,3,2,max_frame,6))
    #print "body_angles len[i][j][k]: %i" % len(all_angles[0][0][0])
    for i, specimen in enumerate(data):
        for j,trc in enumerate(specimen.trc_path):
            for k in range(len(specimen.marker_list)):
                output_data[i,j,k,:len(all_angles[i][j][k]),:] = np.asarray(all_angles[i][j][k])
                data[i].jt_results[j][k] = np.asarray(all_angles[i][j][k])
                data[i].jt_results[j][k][:,3:6] *= 180/np.pi
                if (np.isnan(data[i].jt_results[j][k])).any():
                    print "JT has nan"
                    print data[i].jt_results[j][k]
#    
    #print output_data.shape
    print "JT Analysis Done!"
        
    return output_data
    

def calculate_vertebral_kinematics_mocap(show=True):
    """
    This function will calculate vertebral body translations and rotations
    for each spine, for each trial, for each body.
    
    Inputs are retrieved from the data classes (for specimens 1,2,3)
    output array is a numpy format [3x3x2xNx6] -> [Spine,Trial,Vert,Frame,Dimension]
    
    show is a bool - if True, then the 3x3 plots of the data will be displayed 
    """
    # Load mocap data
    data=[]
    data.append(spine1_data())
    data.append(spine2_data())
    data.append(spine3_data())
    
    # output data [Specimen,Trial,Vertebrae,Frame]
    # initially set to 1 frame and increased to match the number of 
    # mocap frames provided
    max_frame = 1
    #output_data = np.zeros((3,3,max_frame,2))
    # body angles is list that will store data, since we dont know before hand
    # how many mocap frames will be given, we will grow the list dynamically
    # and assign it to the appropriately sized outout_data array, once 
    # all of the calculations are done.  Some bookkeeping: 
    #     body_angles[specimen][trial][vert][frame]
    all_angles = []
    for i, specimen in enumerate(data):
        # iterate over each specimens data class
        all_angles.append([])
        for j,trc in enumerate(specimen.trc_path):
            # iterate over each mocap trial
            all_angles[i].append([])
            print "Processing: %s" % trc
            [n_frames, marker_data, marker_names, data_rate, headers] = marker_data_read(trc)
            if max_frame < n_frames:
                max_frame = n_frames
            # Find static marker position (average first 10 frames)
            # must shift marker indices down by 1 (from marker # to marker index)
            for vert in range(len(specimen.marker_list)):
                P1 = marker_data[:,specimen.marker_list[vert][0]-1,:]
                P2 = marker_data[:,specimen.marker_list[vert][1]-1,:]
                P3 = marker_data[:,specimen.marker_list[vert][2]-1,:]
                angles = simple_kinematics(P1,P2,P3,z_up=True)
                all_angles[i][j].append(angles)
                #pylab.plot(angles)
                #pylab.show()
    
    #all_angles[i][j][vert].append(np.asarray(angles))
    # All calculations are done, put body_angles into an array
    output_data = np.zeros((3,3,2,max_frame,6))
    #print "body_angles len[i][j][k]: %i" % len(all_angles[0][0][0])
    for i, specimen in enumerate(data):
        for j,trc in enumerate(specimen.trc_path):
            for k in range(len(specimen.marker_list)):
                output_data[i,j,k,:len(all_angles[i][j][k]),:] = np.asarray(all_angles[i][j][k])
                # These data are not used anywhere???
                #data[i].mocap_results[j][k] = np.asarray(all_angles[i][j][k])
                #data[i].mocap_results[j][k][:,3:6] *= 180/np.pi
#                
    #print output_data.shape
    print "Analysis Done!"
    
    return output_data

def compare_jt_mocap(DEBUG=2,RET=0,show_fig=False,save_fig=True,save_rms=True):
    """
    """
#===============================================================================
#    Get Data
#===============================================================================
    # Load mocap data
    data=[]
    data1=spine1_data()
    data2=spine2_data()
    data3=spine3_data()
    data.append(data1)
    data.append(data2)
    data.append(data3)
    
    specimens = ['spine1','spine2','spine3']
    trials = ['FE','LB','AR']
    bones = ['vert1','vert2']
    
#===============================================================================
#    Calculate Kinematics
#===============================================================================
    if DEBUG==1:
        # Write the results to file so that they can be quickly reloaded 
        # for future analyses
        print "Calculating all results from sources"
        print "JT analysis"
        jt_results = calculate_vertebral_kinematics_jt(show=False)
        print "Mocap analysis"
        mocap_results = calculate_vertebral_kinematics_mocap(show=False)
        print "Writing results to pickle files"
        jt_fid = open(r'C:\Users\bryan\phd\data\Analysis\jt_results.pickle','w')
        pickle.dump(jt_results, jt_fid)
        jt_fid.close()
        mocap_fid = open(r'C:\Users\bryan\phd\data\Analysis\mocap_results.pickle','w')
        pickle.dump(mocap_results, mocap_fid)
        mocap_fid.close()
    elif DEBUG==2:
        print "Calculating JT results, reading mocap results from pickle files"
        print "JT analysis"
        jt_results = calculate_vertebral_kinematics_jt(show=False)
        jt_fid = open(r'C:\Users\bryan\phd\data\Analysis\jt_results.pickle','w')
        pickle.dump(jt_results, jt_fid)
        jt_fid.close()
        mocap_fid = open(r'C:\Users\bryan\phd\data\Analysis\mocap_results.pickle','r')
        mocap_results = pickle.load(mocap_fid)
        mocap_fid.close()
    elif DEBUG==3:
        print "Reading all results from pickle files"
        jt_fid = open(r'C:\Users\bryan\phd\data\Analysis\jt_results.pickle','r')
        jt_results = pickle.load(jt_fid)
        jt_fid.close()
        mocap_fid = open(r'C:\Users\bryan\phd\data\Analysis\mocap_results.pickle','r')
        mocap_results = pickle.load(mocap_fid)
        mocap_fid.close()
    else:
        jt_results = calculate_vertebral_kinematics_jt(show=False)
        print "Mocap analysis"
        mocap_results = calculate_vertebral_kinematics_mocap(show=False)
    
        
#===============================================================================
#    Synchronize Data
#===============================================================================
    print "Synchronizing data"
    # Create arrays to store synchronized data 
    # (there will be extra frames when finished)
    min_size = np.min(jt_results.shape[3],mocap_results.shape[3])
    error_array = np.zeros((3,3,2,min_size,6))
    rms_out_str = "Spine\tTrial\tVert\tRMS_Xt\tRMS_Yt\tRMS_Zt\tRMS_Xrot\tRMS_Yrot\tRMS_Zrot\n"
    bias_out_str = "Spine\tTrial\tVert\tbias_Xt\tbias_Yt\tbias_Zt\tbias_Xrot\tbias_Yrot\tbias_Zrot\n"
    prec_out_str = "Spine\tTrial\tVert\tprec_Xt\tprec_Yt\tprec_Zt\tprec_Xrot\tprec_Yrot\tprec_Zrot\n"
    jt_out_str = ("Spine\tTrial\tVert\tXt_max\tYt_max\tZt_max\tXrot_max\tYrot_max\tZrot_max\t"
                  "Xt_min\tYt_min\tZt_min\tXrot_min\tYrot_min\tZrot_min\t"
                  "Xt_rom\tYt_rom\tZt_rom\tXrot_rom\tYrot_rom\tZrot_rom\n")
    mocap_out_str = ("Spine\tTrial\tVert\tXt_max\tYt_max\tZt_max\tXrot_max\tYrot_max\tZrot_max\t"
                  "Xt_min\tYt_min\tZt_min\tXrot_min\tYrot_min\tZrot_min\t"
                  "Xt_rom\tYt_rom\tZt_rom\tXrot_rom\tYrot_rom\tZrot_rom\n")
    
    jt_results_sync = np.zeros(jt_results.shape)
    mocap_results_sync = np.zeros(jt_results.shape)
    #error_array_angle = np.zeros((3,3,2,min_size,6))
    
    for i,specimen in enumerate(data):
        # Iterate over each spine
        # add to each data class
        for j,trial in enumerate(trials):
            # Iterate over each trial
            
            sync_fluoro = data[i].sync_fluoro[j]
            sync_mocap = data[i].sync_mocap[j]
            # Take the shorest data set and crop both to be the same 
            # length, synchonized at the same temporal starting frame
            # This would be a spot to optimize if you lack
            # confidence in the sync marker method of temporal synchonization.
            n_frames = np.min((jt_results.shape[3]-sync_fluoro),
                              (mocap_results.shape[3]-sync_mocap))    
#            jt_results_sync[i,j,:,0:n_frames ,:] = jt_results[i,j,:,sync_fluoro:sync_fluoro+n_frames ,:]
#            mocap_results_sync[i,j,:,0:n_frames ,:] = mocap_results[i,j,:,sync_mocap:sync_mocap+n_frames ,:]
            for k,bone in enumerate(bones):
                # Iterate over each vertebrae
                # Synchonize JointTrack
                jt_results_sync[i,j,k,:n_frames,:] = jt_results[i,j,k,sync_fluoro:(sync_fluoro+n_frames),:].copy()
                tmp = jt_results_sync[i,j,k,:,:]
                tmp[np.isnan(tmp)]=0
                data[i].jt_results[j][k] = tmp
                # Synchronize Mocap
                mocap_results_sync[i,j,k,:n_frames,:] = mocap_results[i,j,k,sync_mocap:(sync_mocap+n_frames) ,:].copy()
                tmp = mocap_results_sync[i,j,k,:,:]
                tmp[np.isnan(tmp)]=0
                data[i].mocap_results[j][k] = tmp
                # put error into array for 3x3 plots
                error_array[i,j,k,:n_frames,:] = (mocap_results[i,j,k,sync_mocap:(sync_mocap+n_frames),:] -
                                     jt_results[i,j,k,sync_fluoro:sync_fluoro+n_frames,:]).copy() 

                # DEBUG
                #print "(%i,%i,%i) length JT: %i" % (i,j,k,len(jt_results_sync[i,j,k,:,:]))
                #print "(%i,%i,%i) length Mocap: %i" % (i,j,k,len(mocap_results_sync[i,j,k,:,:]))
#                print "ErrorArray -> Max Error: %f, Min Error: %f" % (error_array[i,j,k,:n_frames,:].max(),
#                                                                      error_array[i,j,k,:n_frames,:].min())
                errors = error_array[i,j,k,:n_frames,:]
                #print "(%i,%i,%i) length Errors: %i" % (i,j,k,len(errors))
                # Convert rotations to degrees
                errors_d = errors.copy()
                errors_d[:,3:6] *= 180/np.pi
                data[i].errors[j][k] = errors.copy()
                
                # Cacluate rms error for each dimension
                data[i].rms[j][k] = JT.Kinematics.array_rms(errors_d)
                
                # Calculate the bias and precision for each dimentsion
                data[i].bias[j][k] = errors_d.mean(0)
                data[i].prec[j][k] = errors_d.std(0)
                
                # Print Results
                rms_vec = data[i].rms[j][k]
                rms_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           rms_vec[0],rms_vec[1],rms_vec[2],
                           rms_vec[3],rms_vec[4],rms_vec[5]) 
                rms_out_str += rms_str
                
                bias_vec = data[i].bias[j][k]
                bias_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           bias_vec[0],bias_vec[1],bias_vec[2],
                           bias_vec[3],bias_vec[4],bias_vec[5]) 
                bias_out_str += bias_str
                
                prec_vec = data[i].prec[j][k]
                prec_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           prec_vec[0],prec_vec[1],prec_vec[2],
                           prec_vec[3],prec_vec[4],prec_vec[5]) 
                prec_out_str += prec_str
                
                jt_max_vec = jt_results_sync[i,j,k,:,:].max(0)
                jt_min_vec = jt_results_sync[i,j,k,:,:].min(0)
                jt_max_vec[3:6]=jt_max_vec[3:6]*180/np.pi
                jt_min_vec[3:6]=jt_min_vec[3:6]*180/np.pi
                jt_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           jt_max_vec[0],jt_max_vec[1],jt_max_vec[2],
                           jt_max_vec[3],jt_max_vec[4],jt_max_vec[5],
                           jt_min_vec[0],jt_min_vec[1],jt_min_vec[2],
                           jt_min_vec[3],jt_min_vec[4],jt_min_vec[5],
                           jt_max_vec[0]-jt_min_vec[0],jt_max_vec[1]-jt_min_vec[1],jt_max_vec[2]-jt_min_vec[2],
                           jt_max_vec[3]-jt_min_vec[3],jt_max_vec[4]-jt_min_vec[4],jt_max_vec[5]-jt_min_vec[5]) 
                jt_out_str += jt_str
                
                mocap_max_vec = mocap_results_sync[i,j,k,:,:].max(0)
                mocap_min_vec = mocap_results_sync[i,j,k,:,:].min(0)
                mocap_max_vec[3:6]=mocap_max_vec[3:6]*180/np.pi
                mocap_min_vec[3:6]=mocap_min_vec[3:6]*180/np.pi
                mocap_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           mocap_max_vec[0],mocap_max_vec[1],mocap_max_vec[2],
                           mocap_max_vec[3],mocap_max_vec[4],mocap_max_vec[5],
                           mocap_min_vec[0],mocap_min_vec[1],mocap_min_vec[2],
                           mocap_min_vec[3],mocap_min_vec[4],mocap_min_vec[5],
                           mocap_max_vec[0]-mocap_min_vec[0],mocap_max_vec[1]-mocap_min_vec[1],mocap_max_vec[2]-mocap_min_vec[2],
                           mocap_max_vec[3]-mocap_min_vec[3],mocap_max_vec[4]-mocap_min_vec[4],mocap_max_vec[5]-mocap_min_vec[5]) 
                mocap_out_str += mocap_str
                
                #print rms_str
                
    print "Calculating Joint Angles"
    jt_joint_angles = np.zeros(jt_results_sync.shape)
    mocap_joint_angles = np.zeros(mocap_results_sync.shape)
    for i in range(len(jt_results_sync)):
        # iterate over specimen
        for j in range(len(jt_results_sync[0,])):
           # iterate over trial
           # assumes joint angles are between bone 1 and 2
           jt_bone1 = jt_results_sync[i,j,0,:,:]
           jt_bone2 = jt_results_sync[i,j,1,:,:]
           mocap_bone1 = mocap_results_sync[i,j,0,:,:]
           mocap_bone2 = mocap_results_sync[i,j,1,:,:]
           print "Joint angles: spine %i, trial %i" % (i,j)
           # store the data in two places: a numpy array of (3,3,2,N,6)  
           # that can be pickled and in the main data structure (dictionary).
           tmp = JT.Kinematics.get_joint_pose(jt_bone1, jt_bone2)
           #tmp[:,3:6] *= 180/np.pi 
           jt_joint_angles[i,j,:,:] = tmp
           data[i].jt_joint_angles[j] = tmp
           
           tmp = JT.Kinematics.get_joint_pose(mocap_bone1, mocap_bone2)
           #tmp[:,3:6] *= 180/np.pi
           mocap_joint_angles[i,j,:,:] = tmp
           data[i].mocap_joint_angles[j] = tmp
    if RET:
        # Write the results to file so that they can be quickly reloaded 
        # for future analyses
        print "Writing results to files"
        fid = open(r'C:\Users\bryan\phd\data\Analysis\all_data.pickle','w')
        #save_data(data,'jt_results',p)
        pickle.dump([jt_results_sync,mocap_results_sync,jt_joint_angles,mocap_joint_angles], fid)
        fid.close()
        print "Done!"
        return [data,jt_results_sync,mocap_results_sync,jt_joint_angles,mocap_joint_angles]
    
           
    if save_rms:
        # Save rms errors to a file for analysis
        rms_filename = r'C:\Users\bryan\phd\data\Analysis\ch3_rms_bias_prec.dat'
        fid = open(rms_filename, 'w')
        fid.write('RMS\n')
        fid.write(rms_out_str)
        fid.write('\nBias\n')
        fid.write(bias_out_str)
        fid.write('\nPrecision\n')
        fid.write(prec_out_str)
        fid.close()

        
        rms_filename = r'C:\Users\bryan\phd\data\Analysis\ch3_rom.dat'
        fid = open(rms_filename, 'w')
        fid.write('pyTrack\n')
        fid.write(jt_out_str)
        fid.write('\nMOCAP\n')
        fid.write(mocap_out_str)
        fid.close()
        
        #rms_angle_filename = os.path.join(data[i].results_path,specimens[i]+trial+bone+'_rms_rotation.dat')
        #save_rms(rms_filename, data[i].rms[trial][bone],'Poses')
        #save_rms(rms_angle_filename, data[i].rms_angle[trial][bone],'Rotations')

    print "Error Analysis"
    print "RMS Errors"
    # Find RMS Error
#===============================================================================
#    Plot
#===============================================================================
    print "Plotting Joint Angles"
    # Joint angles
    [fig_trans_jt_joint,fig_angle_jt_joint] = JT.Kinematics.plot_3x3(data,y_attrib='jt_joint_angles',
                                         x_attrib='frames',bone=None,
                                         y_range=(-10,10),tag='Image Based Joint Angles ',rad=True)
    [fig_trans_mocap_joint,fig_angle_mocap_joint] = JT.Kinematics.plot_3x3(data,y_attrib='mocap_joint_angles',
                                         x_attrib='frames',bone=None,
                                         y_range=(-10,10),tag='Marker Based Joint Angles ',rad=True)
    
    for i in range(2):
        print "Plotting Errors (Bone %i)" % (i+1)
        [fig_trans,fig_angle] = JT.Kinematics.plot_3x3(data,y_attrib='errors',
                                         x_attrib='mocap_results',bone=i,
                                         y_range=(-10,10),tag='Errors Bone '+str(i+1),rad=True)
        [fig_transXframes,fig_angleXframes] = JT.Kinematics.plot_3x3(data,y_attrib='errors',
                                         x_attrib='frames',bone=i,
                                         y_range=(-10,10),tag='Errors Bone '+str(i+1),rad=True)  
        print "Plotting Mocap and JointTrack (Bone %i)" % (i+1)  
        [fig_trans_mocap,fig_angle_mocap] = JT.Kinematics.plot_3x3(data,y_attrib='mocap_results',
                                         x_attrib='frames',bone=i,y_range=(-25,25),
                                         tag='Marker Based - Bone'+str(i+1),rad=True)
        [fig_trans_jt,fig_angle_jt] = JT.Kinematics.plot_3x3(data,y_attrib='jt_results',
                                         x_attrib='frames',bone=i,y_range=(-25,25),
                                         tag='Image Based - Bone'+str(i+1),rad=True)
         
        if save_fig:
            print "Saving figures... (Bone %i)" % (i+1)
            fig_angleXframes.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_errors_vert'+str(i+1)+'_angleXframes.png') 
            fig_transXframes.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_errors_vert'+str(i+1)+'_transXframes.png')
            fig_angle.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_errors_vert'+str(i+1)+'_angleXangle.png') 
            fig_trans.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_errors_vert'+str(i+1)+'_transXangle.png')
            
            fig_angle_mocap.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_mocap_vert'+str(i+1)+'_angles.png')
            fig_trans_mocap.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_mocap_vert'+str(i+1)+'_trans.png')
            
            fig_angle_jt.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_vert'+str(i+1)+'_angles.png')
            fig_trans_jt.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_vert'+str(i+1)+'_trans.png')
            
            if i==0:
                fig_trans_jt_joint.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_joint_trans.png')
                fig_angle_jt_joint.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_joint_angle.png')
                fig_trans_mocap_joint.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_mocap_joint_trans.png')
                fig_angle_mocap_joint.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_mocap_joint_angle.png')
        
    
    if show_fig:
        pylab.show()
    print "Done!"
    
    return data


def sync_all_data():
    print "Reading all results from pickle files"
    jt_fid = open(r'C:\Users\bryan\phd\data\Analysis\jt_results.pickle','r')
    jt_results = pickle.load(jt_fid)
    jt_fid.close()
    mocap_fid = open(r'C:\Users\bryan\phd\data\Analysis\mocap_results.pickle','r')
    mocap_results = pickle.load(mocap_fid)
    mocap_fid.close()
    out_str = 'spine\ttest\tbone\toffset\tR2\n'
    
    for i in range(3):
        #each spine
        for j in range(3):
            #each test
            for k in range(2):
                #each bone
                jt = jt_results[i,j,k,:,3+j]
                mocap = mocap_results[i,j,k,:,3+j]
                [offset,r_max] = JT.Kinematics.synchronize(mocap,jt,False)
                print ("Spine: %i, Test: %i, Bone: %i\n\toffset: %i" 
                       % (i,j,k,offset))
                out_str += '%i\t%i\t%i\t%i\t%f\n' % (i,j,k,offset,r_max)
    
    print out_str
    sync_fid = open(r'C:\Users\bryan\phd\data\Analysis\sync_offsets.txt','w')
    sync_fid.write(out_str)
    sync_fid.close()
    

def save_rms(filename,data,tag=''):
    """
    Given a filename and an array of 2D data, this function will save the
    data as a text file with headers in the first row. 
    """
    data = np.asarray(data)
    n_frames = len(data)
    out_string = 'RMS Error Data - '+tag+'\t# Frames:'+str(n_frames)+'\n'
    out_string += ('Dim\tRMS_Error\n')
    for i in range(data.shape[0]):
        out_string += ('Dim %i\t%f\n' % 
                      (i,data[i]))
    fid = open(filename,'w')
    fid.write(out_string)
    fid.close()



def plot_jt_results():
    """
    Calculate joint angle from pyTrack data and plot results 
    """  
    [joint_angles,vert1_angles,vert2_angles] = calculate_jt_anatomic_joint_angles(save=False,plot=False)
#    
    # Joint Angles 
    [fig_trans_joint,fig_angle_joint] = JT.Kinematics.plot_3x3(joint_angles,tag='JT_joint')
    fig_angle_joint.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_joint_angles.png')
    fig_trans_joint.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_joint_trans.png') 
    # Vert1 Angles 
    [fig_trans_vert1,fig_angle_vert1] = JT.Kinematics.plot_3x3(vert1_angles,tag='JT_vert1')
    fig_angle_vert1.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_vert1_angles.png')
    fig_trans_vert1.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_vert1_trans.png') 
    # Vert1 Angles 
    [fig_trans_vert2,fig_angle_vert2] = JT.Kinematics.plot_3x3(vert2_angles,tag='JT_vert2')
    fig_angle_vert2.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_vert2_angles.png')
    fig_trans_vert2.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_jt_vert2_trans.png')   
    print "Figures saved"
    pylab.show()
    

    
def test_mocap_analysis():    
    sp2_2 = r'C:\Users\bryan\phd\data\Spine2\Spine2_C6C7_markers.xml'
    sp2_5 = r'C:\Users\bryan\phd\data\Spine2\Spine2_5\Spine2_5_C6-C7.xml'
    sp2_9 = r'C:\Users\bryan\phd\data\Spine2\Spine2_9\Spine2_9_C6-C7.xml'
    
#    mocap_analysis(sp2_2,trc2_2)
    data = spine2_setup_data_dict()
    mocap_analysis(data)
#    mocap_analysis(sp2_2,trc2_9)  

def simple_kinematics(P1,P2,P3,z_up=True):
    """
    Use marker locations to define quasi-anatomic reference frame.  Calculate
    kinematics for each vertebrae individually.  
    
    P1,P2,P3 are [N X 3] vectors of marker motions, where N is the number 
    of mocap frames.
    
    A reference frame is constructed from the three markers and the initial 
    10 frames are used a a static 'T-Pose'.
    
    Returns an array of [N x 6] transformations [0:3] and angles(deg) [3:6].
    """
    # All marker arrays must the be same size
    if (len(P1) != len(P2)) or (len(P2) != len(P3)) or (len(P1) != len(P3)):
        print "All marker arrays must be the same size."
        return
     
    P1 = np.asarray(P1)
    P2 = np.asarray(P2)
    P3 = np.asarray(P3)
    
    # Average first 10 frames to define static position
    p1_static = P1[0:10].mean(0)
    p2_static = P2[0:10].mean(0)
    p3_static = P3[0:10].mean(0)
    T_static = JT.Kinematics.build_local(p1_static,
                                         p2_static,
                                         p3_static,z_up)
    
    # Angles is really transfromations and angles
    angles = []
    for i in range(len(P1)):
        # Iterate over each frame in mocap data
        p1_motion = P1[i,:]
        p2_motion = P2[i,:]
        p3_motion = P3[i,:]
        T_motion = JT.Kinematics.build_local(p1_motion,
                                             p2_motion,
                                             p3_motion,z_up)
        
        T_static_motion = np.dot(np.linalg.inv(T_static),T_motion)
        # Find euler angles using 213 [VTK] sequence
        angles.append(JT.Kinematics.get_pose(T_static_motion, '213'))
    
    return np.asarray(angles)

def test_simple_mocap():
    
    # Load mocap data
    data=[]
    data.append(spine1_data())
    data.append(spine2_data())
    data.append(spine3_data())
    
    # output data [Specimen,Trial,Vertebrae,Frame]
    # initially set to 1 frame and increased to match the number of 
    # mocap frames provided
    max_frame = 1
    #output_data = np.zeros((3,3,max_frame,2))
    # body angles is list that will store data, since we dont know before hand
    # how many mocap frames will be given, we will grow the list dynamically
    # and assign it to the appropriately sized outout_data array, once 
    # all of the calculations are done.  Some bookkeeping: 
    #     body_angles[specimen][trial][vert][frame]
    all_angles = []
    for i, specimen in enumerate(data):
        # iterate over each specimens data class
        all_angles.append([])
        for j,trc in enumerate(specimen.trc_path):
            # iterate over each mocap trial
            all_angles[i].append([])
            print "Processing: %s" % trc
            [n_frames, marker_data, marker_names, data_rate, headers] = marker_data_read(trc)
            if max_frame < n_frames:
                max_frame = n_frames
            # Find static marker position (average first 10 frames)
            for vert in range(len(specimen.marker_list)):
                P1 = marker_data[:,specimen.marker_list[vert][0],:]
                P2 = marker_data[:,specimen.marker_list[vert][1],:]
                P3 = marker_data[:,specimen.marker_list[vert][2],:]
                angles = simple_kinematics(P1,P2,P3)
                all_angles[i][j].append(angles)
                #pylab.plot(angles)
                #pylab.show()
    
    #all_angles[i][j][vert].append(np.asarray(angles))
    # All calculations are done, put body_angles into an array
    output_data = np.zeros((3,3,2,max_frame,6))
    print "body_angles len[i][j][k]: %i" % len(all_angles[0][0][0])
    for i, specimen in enumerate(data):
        for j,trc in enumerate(specimen.trc_path):
            for k in range(len(specimen.marker_list)):
                output_data[i,j,k,:len(all_angles[i][j][k]),:] = np.asarray(all_angles[i][j][k])
#                
    #print output_data.shape
    print "Analysis Done!"
    print "Plotting..."
    # output data [Specimen,Trial,Vertebrae,Frame,dimension]
    for i in range(2):
        [fig_trans,fig_angle] = JT.Kinematics.plot_3x3(output_data[:,:,i,:,:],tag='Mocap_vert'+str(i+1))
        fig_angle.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_mocap_vert'+str(i+1)+'_angles.png')
        fig_trans.savefig(r'C:\Users\bryan\phd\data\Analysis\ch3_mocap_vert'+str(i+1)+'_trans.png')
        pylab.show()

def test_simple_jt():
    """
    """
    
    pass

def test_sync():
    
    data=[]
    data1=spine1_data()
    data2=spine2_data()
    data3=spine3_data()
    data.append(data1)
    data.append(data2)
    data.append(data3)
    
    print "Reading all results from pickle files"
    jt_fid = open(r'C:\Users\bryan\phd\data\Analysis\jt_results.pickle','r')
    jt_results = pickle.load(jt_fid)
    jt_fid.close()
    mocap_fid = open(r'C:\Users\bryan\phd\data\Analysis\mocap_results.pickle','r')
    mocap_results = pickle.load(mocap_fid)
    mocap_fid.close()
    for i in range(3):
        #each spine
        for j in range(3):
            #each test
            for k in range(2):
                #each bone
                jt = jt_results[i,j,k,:,3+j]
                mocap = mocap_results[i,j,k,:,3+j]
                
                jt_offset = data[i].sync_fluoro[j]
                mocap_offset = data[i].sync_mocap[j]
                print ("Plotting Spine %i, Test %i, Bone %i (JT: %i, Mocap: %i)"
                       % (i,j,k,jt_offset,mocap_offset))
                fig=pylab.figure()
                ax=fig.add_subplot(111)
            
                ax.plot(jt[jt_offset:])
                ax.plot(mocap[mocap_offset:])

                pylab.ion()
                pylab.show()
    

def test_distance_between_markers():
    # Load mocap data
    data=[]
    data.append(spine1_data())
    data.append(spine2_data())
    data.append(spine3_data())
    for specimen in data:
        for trc_path in specimen.trc_path:
            print trc_path
            for marker_list in specimen.marker_list:
                JT.Kinematics.distance_between_markers(trc_path, marker_list)
    


if __name__ == '__main__':
    #spine2_analysis()
    
#    sync_all_data()
#    create_cluster_model()
#    test_mocap_analysis()
#    test_show_landmarks()
#    test_jt_anatomic_joint_angles()
#    plot_jt_results()
#    test_simple_mocap()
#    test_simple_jt()
#    calculate_vertebral_kinematics_jt()
#    calculate_vertebral_kinematics_mocap()
    compare_jt_mocap(DEBUG=3,RET=1,show_fig=False,save_fig=True,save_rms=True)
#    test_distance_between_markers()
#    test_cost()
#    sync_all_data()
#    test_sync()